/**
 * @file node_monitor.cpp
 * @brief 
 * @author haibo.yang
 * @version 1.1.2
 * @date 2023-08-15
 * 
 * @copyright Copyright (c) 2023  lgim
 * 
 */
#include "state_monitor/safety_monitor.h"
#include "state_monitor/event_manager.h"

#include <agv_msgs/msg/avoid_obsta_info.hpp>
namespace monitor {

void MotorVelocityCurve(CurveObjectType *curve);
static void CalCurveTRAP(CurveObjectType *trap);
static void CalCurveSPTA(CurveObjectType *spta);

void (*pCalCurve[])(CurveObjectType *curve)={nullptr,CalCurveTRAP,CalCurveSPTA};

/* 电机曲线加减速操作-------------------------------------------------------- */
void MotorVelocityCurve(CurveObjectType *curve)
{
	float temp=0;

	if(curve->targetSpeed>curve->speedMax) {
		curve->targetSpeed=curve->speedMax;
	}

	if(curve->targetSpeed<curve->speedMin) {
		curve->targetSpeed=curve->speedMin;
	}

	if((fabs(curve->currentSpeed-curve->startSpeed)<=curve->stepSpeed)&&(curve->maxTimes==0)) {
		if(curve->startSpeed<curve->speedMin) {
			curve->startSpeed=curve->speedMin;
		}

		temp=fabs(curve->targetSpeed-curve->startSpeed);
		temp=temp/curve->stepSpeed;
		curve->maxTimes=(uint32_t)(temp)+1;
		curve->aTimes=0;
	}

	if(curve->aTimes<curve->maxTimes) {
		pCalCurve[curve->curveMode](curve);
		curve->aTimes++;
	} else {
		curve->currentSpeed=curve->targetSpeed;
		curve->maxTimes=0;
		curve->aTimes=0;
	}
}


/**
 * @description: 电机曲线加减速操作
 * @param {CurveObjectType} *curve
 * @return {*}
 */
static void CalCurveTRAP(CurveObjectType *trap)
{
	float slope=0.0;

	slope=(trap->targetSpeed-trap->startSpeed)/trap->maxTimes;

	trap->currentSpeed=trap->startSpeed+slope*trap->aTimes;

	if(trap->currentSpeed>trap->speedMax) {
		trap->currentSpeed=trap->speedMax;
	}

	if(trap->currentSpeed<trap->speedMin) {
		trap->currentSpeed=trap->speedMin;
	}
}
 
/*S型曲线速度计算*/
static void CalCurveSPTA(CurveObjectType *spta)
{
	float power=0.0;
	float speed=0.0;

	power=(2*((float)spta->aTimes)-((float)spta->maxTimes))/((float)spta->maxTimes);
	power=(0.0-spta->flexible)*power;

	speed=1+expf(power);
	speed=(spta->targetSpeed-spta->startSpeed)/speed;
	spta->currentSpeed=speed+spta->startSpeed;

	if(spta->currentSpeed>spta->speedMax) {
		spta->currentSpeed=spta->speedMax;
	}

	if(spta->currentSpeed<spta->speedMin) {
		spta->currentSpeed=spta->speedMin;
	}
}

SafetyMonitor::~SafetyMonitor()
{
	velocity_pub_thread_.join();
	event_pub_thread_.join();
}

void SafetyMonitor::RunOnce(const rclcpp::Time current_time) 
{
	UpdateStatus(current_time);
}

// void SafetyMonitor::SetIoSignal(std::vector<struct IoSignal> &signal, XmlRpc::XmlRpcValue value)
// {
// 	ASSERT(value.size() > 0);
// 	for (const auto &it:value) {
// 		XmlRpc::XmlRpcValue tmp = it.second;
		
// 		struct IoSignal io;
// 		io.name = std::string(tmp["name"]);
// 		io.channel = tmp["channel"];
//         io.polarity = tmp["polarity"];

//         signal.push_back(io);
// 		MONITOR_INFO("[safety monitor] Add signal monitor name : " << io.name << " channel :" << io.channel << " polarity:" << io.polarity);
// 	}
// }

// init 
void SafetyMonitor::init()
{
	// 2024.10.25 by haibo
	MONITOR_INFO("Get safty params ..."); 

	std::string odom_topic;
	io_delay_time_ = Monitor::GetNode()->get_parameter_or("safty_monitor.delay", 5.0);
	odom_topic = Monitor::GetNode()->get_parameter_or("safty_monitor.odom_topic",
		std::string("/odom"));

	MONITOR_INFO("[safety monitor] io delay : " << io_delay_time_);
    GetSlowStopParams();
    
	obstacle_avoid_sub_ = Monitor::GetNode()->create_subscription<agv_msgs::msg::AvoidObstaInfo>(
			"/agv_obstacle_level", 10, 
			std::bind(&SafetyMonitor::ReadObstacleCallBack, this, std::placeholders::_1));

	odom_sub_ = Monitor::GetNode()->create_subscription<agv_msgs::msg::OdometryLite>(
			odom_topic.c_str(), 10, 
			std::bind(&SafetyMonitor::ReadOdomCallBack, this, std::placeholders::_1));

	event_pub_ = Monitor::GetNode()->create_publisher<agv_msgs::msg::AGVEvent>("/saftyevent", 10);

	daemon_vel_pub_ = Monitor::GetNode()->create_publisher<geometry_msgs::msg::TwistStamped>("/twist_mux/daemon_vel", 10);
	realtime_daemon_vel_pub_ =
      	std::make_shared<realtime_tools::RealtimePublisher<geometry_msgs::msg::TwistStamped>>(daemon_vel_pub_);

	// start thread
  	velocity_pub_thread_ = boost::thread(boost::bind(&SafetyMonitor::VelocityPublishhread, this));
  	event_pub_thread_ = boost::thread(boost::bind(&SafetyMonitor::EventPublishThread, this));
}

void SafetyMonitor::GetSlowStopParams()
{
	// 2024.8.27 add slow stop params
	stop_curve_type_[0] = Monitor::GetNode()->get_parameter_or("safty_monitor.EStopMode", (int)CURVE_NONE);
	stop_curve_type_[1] = Monitor::GetNode()->get_parameter_or("safty_monitor.ObsAvoidanceStopMode", (int)CURVE_SPTA);
	stop_curve_type_[2] = Monitor::GetNode()->get_parameter_or("safty_monitor.BumperStopMode", (int)CURVE_NONE);
	speedThreshold_ = Monitor::GetNode()->get_parameter_or("safty_monitor.StopSpeedThreshold", 0.15f);
	curve_.stepSpeed = Monitor::GetNode()->get_parameter_or("safty_monitor.StepSpeed", 0.1f);
  	curve_.speedMax = Monitor::GetNode()->get_parameter_or("safty_monitor.SpeedMax", 1.5f);
  	curve_.speedMin = Monitor::GetNode()->get_parameter_or("safty_monitor.SpeedMin", -1.0f);
  	curve_.flexible = Monitor::GetNode()->get_parameter_or("safty_monitor.Flexible", 5.2f);

    MONITOR_INFO("[safety monitor] stopMode:" << curve_.curveMode << ", stepSpeed:" << curve_.stepSpeed << 
    	", speedMax:" << curve_.speedMax << ", speedMin:" << curve_.speedMin << 
    	", speedThreshold:" << speedThreshold_ << ", flexible:" << curve_.flexible);
}

void SafetyMonitor::VelocityPublishhread()
{
	int interval = 20;
	MONITOR_INFO("[SafetyMonitor] thread1 id:" << boost::this_thread::get_id() << " spid:" << syscall(SYS_gettid));

	while (rclcpp::ok()) {
		// double timestemp = rclcpp::Time::now().toSec();
		PublishEmergencyStop();
		// Sleep for 10 milliseconds
		boost::this_thread::sleep_for(boost::chrono::milliseconds(interval));
	}
}

void SafetyMonitor::EventPublishThread()
{
	int interval = 20;
    MONITOR_INFO("[SafetyMonitor] thread2 id:" << boost::this_thread::get_id() << " spid:" << syscall(SYS_gettid));

	while (rclcpp::ok()) {
		auto current_time = Monitor::GetTimes();
		agv_msgs::msg::AGVEvent event;
		
		event.header.stamp = current_time;

        // int count = 0;
		// if event in queue and publish
		if (!queue_.empty()) {
			while (queue_.size() > 0) {	
				queue_.dequeueEvent(event);
				
		        // publish event
		        PublishSaftyState(event, current_time);
	            boost::this_thread::sleep_for(boost::chrono::milliseconds(1));
	            // count++;
	        } 
            time_last_publish_ = current_time;
	    // } else {
			
		// 	rclcpp::Duration duration(1);
        //     if (current_time - time_last_publish_ < duration.toSec()) {
        //     	double dt = duration.toSec() + time_last_publish_ - current_time;
            	
        //     	boost::this_thread::sleep_for(boost::chrono::milliseconds(int(dt * 1000)));
        //     }

        //     current_time = rclcpp::Time::now().toSec();
		//     PublishSaftyState(event, current_time);
		//     time_last_publish_ = current_time;
		//     continue;
	    // }
        }
		// Sleep for 10 milliseconds
		boost::this_thread::sleep_for(boost::chrono::milliseconds(interval));
	}
}

bool SafetyMonitor::UpdateStatus(const rclcpp::Time time)
{
	CheckSaftyState(time);
	return true;
}

bool SafetyMonitor::SetSaftyState(enum SafetyState newstate)
{
	bool ret = true;
	
	if (pro_state_ < newstate)
		pro_state_ = newstate;

	return ret;
}

void SafetyMonitor::CheckEventActionState()
{
	// check event manager
	AgvEventMap *eventmap = EventManager::Instance()->GetEventMap();
	ASSERT(eventmap != nullptr);
    
    bool servo_event = false; 
    struct EventState state;
	for (auto it = eventmap->begin(); it != eventmap->end(); it++) {
    	AgvEventVector events = it->second;
    	// Using a range-based for loop
    	for (const auto& event : events) {
       		if (event.second.ecode == event.second.CANOPEN_SERVO_ERROR_EVENT) {
	       		servo_event = true;
	       		MONITOR_INFO("[safety monitor] CANOPEN_SERVO_ERROR_EVENT occur!"); 
       		}
    	}
    }
   
    isServoStatusNormal_ = !servo_event;
}

void SafetyMonitor::PublishEmergencyStop()
{
	if (current_state_ == EMERGENCYSTOP || current_state_ == BUMPERSTOP 
		|| current_state_ == EVENTSTOP || current_state_ == OBSTACLESTOP) {
		bool stop_immediately = true;
		if (realtime_daemon_vel_pub_->trylock()) {
			// to do emergency stop
			auto& daemon_vel = realtime_daemon_vel_pub_->msg_;
			rclcpp::Time now = Monitor::GetTimes();
			int slowstopmode = (int)CURVE_NONE;

			switch(current_state_) {
				case EMERGENCYSTOP:
					slowstopmode = stop_curve_type_[0];
					break;
				case OBSTACLESTOP:
					slowstopmode = stop_curve_type_[1];
					break;
				case BUMPERSTOP:
					slowstopmode = stop_curve_type_[2];
					break;
				default:
					break;
			}

			MONITOR_INFO("[safety monitor] slowstopmode:" << slowstopmode);
			if (slowstopmode > (int)CURVE_NONE && (now - speed_.lastupdate < rclcpp::Duration(0, 0.1 * 1e9))) {
				float abs_real_velocity = std::max(fabs(speed_.cur_speed[0]), std::max(fabs(speed_.cur_speed[1]), fabs(speed_.cur_speed[2])));
				MONITOR_WARN("[safety monitor] abs_real_velocity:" << abs_real_velocity);
                
				if (abs_real_velocity > speedThreshold_ && 
					abs_real_velocity <= curve_.speedMax && 
					isServoStatusNormal_ && now - prevSpeedAdjustTimestamp_ > rclcpp::Duration(1, 0)) {
					stop_immediately = false;
				}

				if (curve_.maxTimes > 0) {
					stop_immediately = false;
				} else if (!stop_immediately && curve_.maxTimes == 0) {
					double maxAbsValue = 0;
					speed_.index = 0;
					curve_.curveMode = (SpeedCurveType)slowstopmode;
					// get params 
					GetSlowStopParams();

					for (int i=0;i<3;i++) {
						if (maxAbsValue < fabs(speed_.cur_speed[i])) {
							speed_.index = i;
							maxAbsValue = fabs(speed_.cur_speed[i]);
						}
						speed_.ins_speed[i] = speed_.cur_speed[i];
					}

					curve_.startSpeed = maxAbsValue;
 					curve_.targetSpeed = 0.0f;
				 	curve_.currentSpeed = maxAbsValue;

				 	prevSpeedAdjustTimestamp_ = now;
				 	MONITOR_WARN("[safety monitor] set curve startSpeed:" << curve_.startSpeed);
				}
			}

			if (stop_immediately) {
				daemon_vel.twist.linear.x = 0.0f;
				daemon_vel.twist.linear.y = 0.0f;
				daemon_vel.twist.angular.z = 0.0f;
			} else {
				MotorVelocityCurve(&curve_);
				double scale = curve_.currentSpeed / fabs(speed_.ins_speed[speed_.index]);
				if (scale > 1.0f) {
					daemon_vel.twist.linear.x = 0.0f;
					daemon_vel.twist.linear.y = 0.0f;
					daemon_vel.twist.angular.z = 0.0f;
				} else {
					daemon_vel.twist.linear.x = speed_.ins_speed[0] * scale;
					daemon_vel.twist.linear.y = speed_.ins_speed[1] * scale;;
					daemon_vel.twist.angular.z = speed_.ins_speed[2] * scale;;
				}

				MONITOR_WARN("[safety monitor] sacle:" << scale << ",set curve currentSpeed:" << daemon_vel.twist.linear.x << 
					"," << daemon_vel.twist.linear.y << "," << daemon_vel.twist.angular.z << ",current velocity:" << 
					speed_.cur_speed[0] << "," << speed_.cur_speed[1] << "," << speed_.cur_speed[2]);
			}

	        daemon_vel.twist.linear.z = 0.0f;
			daemon_vel.twist.angular.x = 0.0f;
			daemon_vel.twist.angular.y = 0.0f;
	        
			realtime_daemon_vel_pub_->unlockAndPublish();
			MONITOR_INFO("[safety monitor] Emergency stop publish");
		}
	} else {
		curve_.reset();
	}
}

void SafetyMonitor::PublishSaftyState(agv_msgs::msg::AGVEvent& event, const rclcpp::Time time)
{
	event.header.frame_id = "safteyevent";
    event.header.stamp = time;

	event_pub_->publish(event);
	// MONITOR_INFO("[safety monitor] publish saftyevent: " << event);
	
	time_last_publish_ = time;
	// Prevent event loopback
	if (current_state_ != EVENTSTOP) {
		agv_msgs::msg::AGVEvent::SharedPtr eventPtr = std::make_shared<agv_msgs::msg::AGVEvent>(event);
		EventManager::EscalateEvents("saftymonitor", eventPtr);
	}
}

void SafetyMonitor::CheckSaftyState(const rclcpp::Time time)
{
	pro_state_ = NORMAL;
    
    ReadInput(time);

    // check io reset signal
    bool is_reset = false;
    // check io emergency stop
	bool is_emergencystop = false;
	// check bumper stop
	bool is_bumperstop = false;
	// check io stop
	bool is_iostop = false;
	// check io pause 
	bool is_iopause = false;
	bool clear_reset = false;

	if (!safetyevent_.io.IsTimeOut(time)) {

	    // for (auto iter : safetyevent_.io.reset) {
		// 	if (iter.level) {
		// 		is_reset = true;
		// 	}
		// }
		
		if (safetyevent_.io.reset.triggered) {
			if (!safetyevent_.io.reset.isTimeout(time))
				is_reset = true;
			else 
				safetyevent_.io.reset.clear();
		}
		

		// for (auto iter : safetyevent_.io.emergencystop) {
			if (safetyevent_.io.emergencystop.level) {
				is_emergencystop = true;
				agv_msgs::msg::AGVEvent event;
				event.type = event.ALARM;
				event.action = event.ESTOP;
				event.ackreq = true;
			    event.ecode = event.EMG_STOP_EVENT;
			    event.eventtime = safetyevent_.io.emergencystop.eventtime.seconds();
				// event.param1 = iter.channel;
				event.description = safetyevent_.io.emergencystop.name;//iter.name;
				queue_.enqueueEvent(event);
				// break;
			}
		// }

		if (is_emergencystop) {
			SetSaftyState(EMERGENCYSTOP);
		} else if (current_state_ == EMERGENCYSTOP) {
			agv_msgs::msg::AGVEvent event;
			event.type = event.ALARM;
			event.action = event.ESTOP;
			event.ackreq = true;
		    event.ecode = event.EMG_STOP_EVENT;
		    event.eventtime = 0;
			event.param1 = 0;
			event.description = "Event cleared, need push reset button";
			queue_.enqueueEvent(event);
		}

		// for (auto iter : safetyevent_.io.bumper) {
			if (safetyevent_.io.bumper.level) {
				is_bumperstop = true;
				agv_msgs::msg::AGVEvent event;
				event.type = event.ALARM;
				event.action = event.ESTOP;
				event.ackreq = true;
			    event.ecode = event.BUMPER_EVENT;
			    event.eventtime = safetyevent_.io.bumper.eventtime.seconds();
				// event.param1 = iter.channel;
				event.description = safetyevent_.io.bumper.name;//iter.name;
				queue_.enqueueEvent(event);
				// break;
			}
		// }

		if (is_bumperstop) {
			SetSaftyState(BUMPERSTOP);
		} else if (current_state_ == BUMPERSTOP) {
			agv_msgs::msg::AGVEvent event;
			event.type = event.ALARM;
			event.action = event.ESTOP;
			event.ackreq = true;
		    event.ecode = event.BUMPER_EVENT;
		    event.eventtime = 0;
			event.param1 = 0;
			event.description = "Event cleared, need push reset button";
			queue_.enqueueEvent(event);
		}

	   	// for (auto &iter : safetyevent_.io.stop) {
   			if (safetyevent_.io.stop.level || safetyevent_.io.stop.triggered) {
   				if (is_reset && safetyevent_.io.stop.triggered) {
   					safetyevent_.io.stop.clear();
   				} else {
	   				is_iostop = true;
	   				agv_msgs::msg::AGVEvent event;
	   				event.type = event.ALARM;
					event.action = event.STOP;
					event.ackreq = false;
				    event.ecode = event.RUN_STOP_EVENT;
				    event.eventtime = safetyevent_.io.stop.eventtime.seconds();
					// event.param1 = iter.channel;
					event.description = safetyevent_.io.stop.name;//iter.name;
					queue_.enqueueEvent(event);
				}
   				// break;
			}	
		// }

		if (is_iostop) {
			SetSaftyState(STOP);
		}

		// for (auto &iter : safetyevent_.io.pause) {
			if (safetyevent_.io.pause.level || safetyevent_.io.pause.triggered) {
				if (is_reset && safetyevent_.io.pause.triggered) {
   					safetyevent_.io.pause.clear();
   				} else {
					is_iopause = true;
				    agv_msgs::msg::AGVEvent event;
					event.type = event.ALARM;
					event.action = event.STOP;
					event.ackreq = false;
					event.ecode = event.SAFETY_PAUSE;
				    event.eventtime = safetyevent_.io.pause.eventtime.seconds();
					// event.param1 = safetyevent_.io.pause.channel;
					event.description = safetyevent_.io.pause.name;//iter.name;
					queue_.enqueueEvent(event);
					// break;
				}
			}	
		// }

		if (is_iopause) {
			SetSaftyState(PAUSE);
		}
	} else if (!sim_mode_) {
	    agv_msgs::msg::AGVEvent event;
		event.type = event.ALARM;
		event.action = event.STOP;
		event.ackreq = false;
		event.eventtime = safetyevent_.io.lastupdate.seconds();
	    event.ecode = event.IO_TIME_OUT;
	    event.description = "IO time out";
	    queue_.enqueueEvent(event);
		SetSaftyState(IOTIMEOUT);
	}

	// check event
    CheckEventActionState();
	// if (safetyevent_.eventstate.action == ACTION_ESTOP) {
	// 	SetSaftyState(EVENTSTOP);
	// }
   
	// check obstacle stop
	if (!safetyevent_.obstacle.IsTimeOut(time)) { 
		if(safetyevent_.obstacle.stop.first) {
			agv_msgs::msg::AGVEvent event;
			event.type = event.ALARM;
			event.action = event.ESTOP;
			event.ackreq = false;
			event.eventtime = safetyevent_.obstacle.eventtime.seconds();
		    event.ecode = event.SAFE_DETECT_NEAR_EVENT;
		    event.param1 = safetyevent_.obstacle.stop.second[0].first;
		    event.param2 = safetyevent_.obstacle.stop.second[0].second; 
		    event.description = "scenesname: " + safetyevent_.obstacle.scenesname + " obstacle stop";
		    queue_.enqueueEvent(event);
			SetSaftyState(OBSTACLESTOP);
		}

		// check obstacle slow
		if (safetyevent_.obstacle.slow.first) {
			agv_msgs::msg::AGVEvent event;
			event.type = event.WARNING;
			event.action = event.SLOWDONE;
			event.ackreq = false;
			event.eventtime = safetyevent_.obstacle.eventtime.seconds();
			event.vellimit = 0.3f;
		    event.ecode = event.SAFE_DETECT_CENTER_EVENT;
		    event.param1 = safetyevent_.obstacle.slow.second[0].first;
		    event.param2 = safetyevent_.obstacle.slow.second[0].second; 
		    event.description = "scenesname: " + safetyevent_.obstacle.scenesname + " obstacle slow";
		    queue_.enqueueEvent(event);
			SetSaftyState(SLOW);
		}

		// check obstacle warn
	 	if (safetyevent_.obstacle.warn.first) {
	 		agv_msgs::msg::AGVEvent event;
	 		event.type = event.INFO;
			event.action = event.NONE;
			event.ackreq = false;
			event.eventtime = safetyevent_.obstacle.eventtime.seconds();
		    event.ecode = event.SAFE_DETECT_FAR_EVENT;
		    event.param1 = safetyevent_.obstacle.warn.second[0].first;
		    event.param2 = safetyevent_.obstacle.warn.second[0].second;
		    event.description = "scenesname: " + safetyevent_.obstacle.scenesname + " obstacle warn";
		    queue_.enqueueEvent(event);
			SetSaftyState(WARN);
		}
	} else if (!sim_mode_) {
		// MONITOR_ERROR("[safety monitor] State Error:signal obstacle time out!");
		agv_msgs::msg::AGVEvent event;
		event.type = event.ALARM;
		event.action = event.STOP;
		event.ackreq = false;
		event.eventtime = safetyevent_.obstacle.lastupdate.seconds();
	    event.ecode = event.OBSTACLE_AVOID_TIME_OUT;
	    event.description = "obstacle time out";
	    queue_.enqueueEvent(event);
		SetSaftyState(OBSTACLETIMEOUT);
	}

	// state switch
	switch(current_state_) {
		case NORMAL:
		case PAUSE:
		case WARN:
		case SLOW:
		case OBSTACLESTOP:
    	case EVENTSTOP:
    	case IOTIMEOUT:
    	case OBSTACLETIMEOUT: 
    	case STOP:
    		if (current_state_ != pro_state_) {
    			if (current_state_ == OBSTACLESTOP) {
    				curve_.reset();
    			}
				current_state_ = pro_state_;
				MONITOR_INFO("[safety monitor] Change current state: " << current_state_ << ", new state :" << pro_state_);
			}
			break;	
		case BUMPERSTOP:
		case EMERGENCYSTOP:
			if (pro_state_ != EMERGENCYSTOP && pro_state_ != BUMPERSTOP && pro_state_ != STOP) {
				// for (auto iter : safetyevent_.io.reset) {
					if (is_reset) {
						current_state_ = pro_state_;
						MONITOR_INFO("[safety monitor] Change current state: " << current_state_ << ", new state :" << pro_state_);
					}
				// }
			} else {
				if (current_state_ != pro_state_) {
					current_state_ = pro_state_;
					MONITOR_INFO("[safety monitor] Change current state: " << current_state_ << ", new state :" << pro_state_);
				}
			}
			break;
		default:
			break;
	}

	monitor::Monitor::SetVehicleSaftyState((int)current_state_);
}


void SafetyMonitor::ReadInput(rclcpp::Time time)
{
    if (wait_io_) {
    	auto ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
                std::chrono::duration<double>(io_delay_time_));

    	if (time - start_time_ >= rclcpp::Duration(ns)) {
	    	wait_io_ = false;
	    	MONITOR_INFO("[safty monitor] stop wait io module, delay :" << (time.seconds() - start_time_.seconds())); 
    	} else {
    		return;
    	}
    }

	safetyevent_.io.lastupdate = Monitor::inputs.first;
	std::vector<agv_msgs::msg::PairTypeInt> inputs_msg = Monitor::inputs.second.array;
	bool bumper = false;
	bool reset = false;
	bool estop = false;
	bool stop = false;
	bool pause = false;
	
	for (auto &data : inputs_msg) {
		if (data.key == "bumper") {
			bumper = (data.value == IO_LEVEL_HIGHT);
		} else if(data.key == "emergencyButton") {
			estop = (data.value == IO_LEVEL_HIGHT);
		} else if(data.key == "resetButton") {
			reset = (data.value == IO_LEVEL_HIGHT);
		} else if(data.key == "stopButton") {
			stop = (data.value == IO_LEVEL_HIGHT);
		} else if(data.key == "pauseButton") {
			pause = (data.value == IO_LEVEL_HIGHT);
		} 
	}    

	safetyevent_.io.bumper.set(bumper, Monitor::inputs.first);   
	safetyevent_.io.emergencystop.set(estop, Monitor::inputs.first);
	safetyevent_.io.reset.set(reset, Monitor::inputs.first);
	safetyevent_.io.stop.set(stop, Monitor::inputs.first);
	safetyevent_.io.pause.set(pause, Monitor::inputs.first);
}

// bool SafetyMonitor::CheckIoFromInputs(int channel, bool polarity, const uint64_t &msg_bits)
// {
// 	return (TestBit(msg_bits, channel) == polarity);
// }

void SafetyMonitor::ReadObstacleCallBack(const agv_msgs::msg::AvoidObstaInfo &msg)
{   
	// MONITOR_INFO("[safety monitor] Get obstacle :" << msg);
    rclcpp::Time timestamp =  Monitor::GetNode()->now(); // Get current timestamp
    
    auto sensorlist = msg.avoid_msg;
    safetyevent_.resetObstacle();

    safetyevent_.obstacle.eventtime =  rclcpp::Time(msg.time.sec, msg.time.nanosec);
    safetyevent_.obstacle.scenesname = msg.scenes_name;
    
    bool is_warn = false, is_slow = false, is_stop = false;
    for (std::size_t i = 0; i < sensorlist.size(); i++) {
        auto& sensor = sensorlist[i];
        
        if (sensor.warn) {
        	is_warn = true;
	        AlarmRegion region;
	        region.first = sensor.equipment;
	        region.second = sensor.area;
	        
	        safetyevent_.obstacle.warn.second.push_back(region);
	        MONITOR_INFO("[safety monitor] Obstacle equipment[" << i << "]:" << (uint)(sensor.equipment) << ",area:" << (uint)(sensor.area)
	                                                        << " warn is occur");
        }

        if (sensor.slow) {
        	is_slow = true;
            AlarmRegion region;
	        region.first = sensor.equipment;
	        region.second = sensor.area;
    		
	        safetyevent_.obstacle.slow.second.push_back(region);
            MONITOR_INFO("[safety monitor] Obstacle equipment[" << i << "]:" << (uint)(sensor.equipment) << ",area:" << (uint)(sensor.area)
                                                            << " slow is occur");
        }

        if (sensor.stop) {
        	is_stop = true;
        	AlarmRegion region;
	        region.first = sensor.equipment;
	        region.second = sensor.area;

	        safetyevent_.obstacle.stop.second.push_back(region);
            MONITOR_INFO("[safety monitor] Obstacle equipment[" << i << "]:" << (uint)(sensor.equipment) << ",area:" << (uint)(sensor.area)
                                                            << " stop is occur");
        }
    }
    safetyevent_.obstacle.count++;
    safetyevent_.obstacle.lastupdate = timestamp;
    safetyevent_.obstacle.warn.first = is_warn;
    safetyevent_.obstacle.slow.first = is_slow;
    safetyevent_.obstacle.stop.first = is_stop;
}

void SafetyMonitor::ReadOdomCallBack(const agv_msgs::msg::OdometryLite &msg)
{
	speed_.cur_speed[0] = msg.twist.linear.x;
	speed_.cur_speed[1] = msg.twist.linear.y;
	speed_.cur_speed[2] = msg.twist.angular.z;

	speed_.lastupdate = Monitor::GetNode()->now();
}

} // end namespace
